#include "../globals/globals.h"
#include "../pid/pid.h"
#include "../_includes.h"
void fast_forward_dist_relative_cb_static();

struct Arg_fast_forward {
  bool run_done;  // 状态标志，是否完成
  double aim_dist_mm;
  double max_pwr;
  int timeout;
  bool waitForCompletion;
  double max_adjust_pwr;
} arg_fast_forward;

bool fast_forward_force_stop = false;
bool fast_forward_running = false;
/////////////////////////////////////////////////////////////////////
void fast_forward_stop() {
  fast_forward_force_stop = true;
  while_wait(fast_forward_running);
  fast_forward_force_stop = false;
}
bool is_fast_forward_done() { return arg_fast_forward.run_done; }

//////////////////////////////////////////////////////////////// ///////////////
/*
启动函数
*/
///////////////////////////////////////////////////////////////////////////////
void fast_forward_dist_relative(double aim_dist_mm, double max_pwr, int timeout,
                                bool waitForCompletion, double max_adjust_pwr) {
  // 等待前一個线程结束
  while (fast_forward_running) wait(5);

  arg_fast_forward = {false,   aim_dist_mm,       max_pwr,
                      timeout, waitForCompletion, max_adjust_pwr};

  fast_forward_force_stop = false;

  // 建立线程
  thread t(fast_forward_dist_relative_cb_static);

  // 等待启动
  while (!fast_forward_running) {
    wait(5);
  }

  // 等待完成
  if (waitForCompletion) {
    while (fast_forward_running && !fast_forward_force_stop) wait(20);
  }
}
////////////////////////////////////////////////////////////////////////////////
/*
线程执行函数
*/
void fast_forward_dist_relative_cb_static() {
  fast_forward_running = true;
  wait(10);  // cpu shift

  // 建立局部变量，供本线程使用
  double aim_dist_mm = arg_fast_forward.aim_dist_mm;
  int timeout = arg_fast_forward.timeout;
  double max_adjust_pwr = arg_fast_forward.max_adjust_pwr;
  double max_pwr = arg_fast_forward.max_pwr;
  ///////////////////////////////////////////////////
  int dist_beg_l = chassis.get_dist_mm(-1);
  int dist_beg_r = chassis.get_dist_mm(1);

  int dir = 1;
  if (aim_dist_mm < 0) dir = -1;
  if (timeout == 0) timeout = 1000000;  // long time enough

  pid_ctrl pid = pid_ctrl::get_instance_old_version(1.5, 0.01, 0, 5, 0, 5, 10,
                                                    max_adjust_pwr);
  double turn_adj, fwd_adj;
  timer t;

  double rate = 0.1;  // 缓加速

  while (!fast_forward_force_stop && t.time() < timeout &&
         abs(chassis.get_dist_mm(-1) - dist_beg_l) < abs(aim_dist_mm)) {
    turn_adj = pid.pid_output_need_value(chassis.get_dist_mm(-1) - dist_beg_l,
                                         chassis.get_dist_mm(1) - dist_beg_r);

    fwd_adj = max_pwr - abs(turn_adj);

    // 缓加速
    fwd_adj *= rate;
    if (abs(fwd_adj) < 20) fwd_adj = sgn(fwd_adj) * 20;
    if (rate < 1) rate += 0.05;

    chassis.FT_set_voltage(dir * fwd_adj, -turn_adj);
    // cout << fwd_adj << endl;
    // cout << turn_adj << endl << endl;

    wait(5);
  }
  chassis.mtr_stop(brakeType::hold);
  if (!fast_forward_force_stop) wait(100);
  fast_forward_running = false;
  arg_fast_forward.run_done = true;
}


/*
--------------------------------------------------------------
弥补橙色车缺陷重新写的
--------------------------------------------------------------
*/

#define deg2dist 0.609247//0.48024226//mm/deg


#define Ei_line_max 4

#define brake_ddz 0
#define turn_gain 0.3

#define I_startpoint 150

#define Kd_line 0.05

float Ep_line=0,Ei_line=0,Ed_line=0;
float L_rot=0,R_rot=0;

float output_line_form=0;

float output_line_R=0,output_line_L=0;
float error_line=0;
int line_flag=0;

float line_aim=0,line_speed=0;
int loop_line=0;

  //int dist_beg_l= chassis.get_dist_mm(-1);
  //int dist_beg_r = chassis.get_dist_mm(1);


void costume_go(double aim,double speed,double timeout,double k_gain,int quick_exit)
{
  quick_exit=2-quick_exit;

  vex::timer systic_line;
  double Ki_line,Kp_line;

    line_flag=1;
    loop_line=0;
    mtr_LF.resetPosition();
    mtr_LB.resetPosition();
    mtr_RF.resetPosition();
    mtr_RF.resetPosition();
    line_aim=aim/deg2dist*360;
    line_speed=speed;
    systic_line.clear();

    Ki_line=0.01;//*k_gain;
    Kp_line=0.6/sqrt(abs(aim))*k_gain+0.03*k_gain;


       

    while(systic_line.time()<=timeout&&quick_exit>0)
    {
        line_loop(aim,Kp_line,Ki_line,quick_exit);
        line_output();
        wait(5,msec);
    }
    chassis.mtr_stop(coast);
    if(quick_exit==1)
    {
      chassis.mtr_stop(hold);
    line_flag=0;
    }
      
    cout <<systic_line.time()<< endl;
    cout <<deg2dist*(mtr_LF.position(rotationUnits::deg)+mtr_LB.position(rotationUnits::deg))/2<< endl;
    cout <<deg2dist*(mtr_RF.position(rotationUnits::deg)+mtr_RF.position(rotationUnits::deg))/2<< endl;
}


pid_ctrl pid = pid_ctrl::get_instance_old_version(1.3, 0.01, 0, 5, 0, 5, 10,
                                                    100);

void line_loop(double aim,float Kp_line,float Ki_line,int quick_exit)
{

    loop_line++;
    L_rot=deg2dist*(mtr_LF.position(rotationUnits::deg)+mtr_LB.position(rotationUnits::deg))/2;
    R_rot=deg2dist*(mtr_RF.position(rotationUnits::deg)+mtr_RF.position(rotationUnits::deg))/2;
    // error_R=aim-R_rot;
    // error_L=aim-L_rot;


    error_line=aim-(R_rot+L_rot)/2;

    //error_line=aim-L_rot;
    Ep_line=Kp_line*error_line;
    Ei_line+=Ki_line*error_line;
    if(abs(Ei_line)>=Ei_line_max)
    {
        Ei_line=sgn(Ei_line)*Ei_line_max;
    }
    if(error_line*Ei_line<0)
    {
        Ei_line=error_line;
    }
    if(error_line>I_startpoint)
      Ei_line=0;
    double turn_adj;
  //   int dist_beg_l;// = chassis.get_dist_mm(-1);
  //  int dist_beg_r;// = chassis.get_dist_mm(1);
    
    turn_adj = pid.pid_output_need_value(L_rot,R_rot);


    turn_adj*=turn_gain*((error_line)/aim+0.15);
    if(Ei_line>0)
    Ei_line*=((error_line)/I_startpoint+0.16);
    else
    Ei_line*=0.5;
    
    Ep_line*=((error_line-20)/aim+0.1)*1.5;
    output_line_L=Ep_line+Ei_line-turn_adj;
    
    output_line_R=Ep_line+Ei_line+turn_adj;


    // if(error_line<50)
    //   quick_exit--;
    




    // error_line=aim-R_rot;
    // Ep_line=Kp_line*error_line;
    // Ei_line+=Ki_line*error_line;
    // if(abs(Ei_line)>=Ei_line_max)
    // {
    //     Ei_line=sgn(Ei_line)*Ei_line_max;
    // }
    // if(error_line*Ei_line<0)
    // {
    //     Ei_line=error_line;
    // }
    // output_line_R=Ep_line+Ei_line;
    // output_line_R*=(error_line/aim+0.25);





   // output_line_L=Ep_line+Ei_line;
  //  if(error_line<brake_ddz)
  //  {
  //   line_flag=0;
  //  }
  //  else
  //   line_flag=1;

}


void line_output()
{
    if(line_flag)
    {
    // output_line_L*=10;//12.8/100;
    // output_line_R*=10;//12.8/100;
    mtr_LF.spin(directionType::fwd,output_line_L,voltageUnits::volt);  
    mtr_LB.spin(directionType::fwd,output_line_L,voltageUnits::volt);  
    mtr_LF1.spin(directionType::fwd,output_line_L,voltageUnits::volt); 
    mtr_LB1.spin(directionType::fwd,output_line_L,voltageUnits::volt);
    mtr_RF.spin(directionType::fwd,output_line_R,voltageUnits::volt);  
    mtr_RB.spin(directionType::fwd,output_line_R,voltageUnits::volt);  
    mtr_RF1.spin(directionType::fwd,output_line_R,voltageUnits::volt);  
    mtr_RB1.spin(directionType::fwd,output_line_R,voltageUnits::volt); 


    }
    else
    {
        chassis.mtr_stop(brake);
    }
}